import cv2
import numpy as np


def picture(image, disparity, path, name):
    # 将视差图转换为伪彩色图像
    disparity_color = cv2.applyColorMap(cv2.normalize(disparity, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8),
                                        cv2.COLORMAP_JET)

    # 将原图转换为彩色（如果它是灰度图）
    if len(image.shape) == 2:
        image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)

    # 融合视差图和彩色原图
    alpha = 0.8  # 视差图的权重
    beta = 0.2  # 原图的权重
    blended_image = cv2.addWeighted(image, beta, disparity_color, alpha, 0)

    # 显示融合图像
    cv2.imshow(name, blended_image)
    cv2.imwrite(path, blended_image)
    cv2.waitKey()
    cv2.destroyAllWindows()


# 读取左右图像
# gray_left = cv2.imread(r"D:\Project\binocularCamera\data\output_719\left_camera.png")
# gray_right = cv2.imread(r"D:\Project\binocularCamera\data\output_719\right_camera.png")
gray_left = cv2.imread(r"E:\Blender\output\output_blender_test\left_camera.png")
gray_right = cv2.imread(r"E:\Blender\output\output_blender_test\right_camera.png")

# 创建 SGBM 对象
block_size = 3
min_disp = 0
num_disp = 16 * 7
img_channels = 3

parmL = {'minDisparity': 0,
         'numDisparities': 16,
         'blockSize': block_size,
         'P1': 8 * img_channels * block_size ** 2,
         'P2': 32 * img_channels * block_size ** 2,
         'disp12MaxDiff': 1,
         'preFilterCap': 31,
         'uniquenessRatio': 15,
         'speckleWindowSize': 100,
         'speckleRange': 1,
         'mode': cv2.STEREO_SGBM_MODE_SGBM
         }

paramR = {'minDisparity': 0,
          'numDisparities': 16,
          'blockSize': block_size,
          'P1': 8 * img_channels * block_size ** 2,
          'P2': 32 * img_channels * block_size ** 2,
          'disp12MaxDiff': 1,
          'preFilterCap': 31,
          'uniquenessRatio': 15,
          'speckleWindowSize': 100,
          'speckleRange': 1,
          'mode': cv2.STEREO_SGBM_MODE_SGBM
          }

stereoL = cv2.StereoSGBM_create(**parmL)
disparityLR = stereoL.compute(gray_left, gray_right).astype(np.float32) / 16.0

print(disparityLR[555][539])

stereoR = cv2.StereoSGBM_create(**paramR)
disparityRL = stereoR.compute(gray_right, gray_left).astype(np.float32) / 16.0
print(disparityRL[555][539])


picture(gray_left, disparityLR, r'D:\Project\binocularCamera\gbl\res\LR.png', 'LR')
picture(gray_right, disparityRL, r'D:\Project\binocularCamera\gbl\res\RL.png', 'RL')

